#ifndef ARAL_INTERFACE_UTILITY_HPP
#define ARAL_INTERFACE_UTILITY_HPP

#include "aral/data_structure_definition.hpp"
#include <string>
#include <utility>
#include <memory>

namespace ARAL::interface {

/**
 * @class Utility
 * @brief 提供独立于特定机器人模型的通用算法和工具。
 * 
 * @details
 *  - **通用工具集**: 包含日志管理、配置、版本信息、错误处理、数学计算、滤波器等与核心场景管理无关的通用功能。
 *  - **独立模块**: 作为一个独立的模块，由 Scene 类创建并持有，用户通过 `scene->getUtility()` 获取。
 * 
 * @b 使用方法:
 * @code
 * auto scene = CreateARALScene();
 * auto utility = scene->getUtility();
 * 
 * utility->rlSetLogLevel(LogLevel::DEBUG);
 * auto version = utility->rlGetARALVersionNumber();
 * std::cout << "ARAL Version: " << version << std::endl;
 * @endcode
 */
class Utility
{
protected:
    /**
     * @brief 虚析构函数。
     */
    virtual ~Utility() = default;

public:
    /**
     * @brief 设置场景的日志打印级别。
     * @details 算法库只打印不大于`level`级别的日志，默认等级为`info`。
     * @param level 日志级别: 0:fatal; 1:error; 2:warn; 3:info; 4:debug; 5:verbose
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int rlSetLogLevel(const LogLevel& level) = 0;

    /**
     * @brief 设置自定义日志处理函数。
     * @param logFunc 日志处理回调函数。如果未设置, 默认使用`printf`输出到标准输出。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int rlSetLogHandler(const LogHandler& logFunc) = 0;

    /**
     * @brief 获取错误码对应的详细错误描述。
     * @param errCode 错误码。
     * @return 错误的详细描述字符串。
     */
    virtual const std::string rlGetErrorDescription(const int errCode)const = 0;

    /**
     * @brief 获得ARAL库的版本信息。
     * @return 整数形式的版本号 (例如: 1002003 代表 v1.2.3)。
     */
    virtual unsigned int rlGetARALVersionNumber()const = 0;

    /**
     * @brief 通过键值对来配置算法库的特性。
     * @param config 键值对, 第一个元素为配置名称, 第二个元素为具体的值。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int rlConfigureLibrary(const std::pair<std::string, aral_any>& config) = 0;

    /**
     * @brief 获取算法库的当前所有配置参数。
     * @return 返回只读的配置参数映射表。
     */
    virtual const ConfigMap& rlGetConfiguration() const = 0;

    /**
     * @brief 计算向量的模值
     * @param data: 向量的数据
     * @param size: 向量的长度
     * @return: 向量的模值
     */
    virtual double utlVectorNorm(const double* data, const int& size)const = 0;

    /**
     * @brief 一阶低通滤波器
     * @param sample_time: 信号采样周期
     * @param cutoff_frequency: 滤波器的截止频率
     * @param size: 输入状态维度
     * @param x_last: 滤波器在上一时刻的状态输出
     * @param x: 当前需要被滤波的状态及当前输出
     * @return if < 0, 如果 cutoff_frequency sample_time 小于等于 0
     */
    virtual int utlLowpassFilter(const double& sample_time, const double& cutoff_frequency, const int& size, const double* x_last, double* x) = 0;

    /**
     * @brief 卡尔曼滤波器(无模型)
     * @param Q: 控制过程误差
     * @param R: 信号测量方差
     * @param x_last: 滤波器在上一时刻的状态输出
     * @param x: 当前需要被滤波的状态及当前输出
     * @param P: 后验估计方差(初值可设为1)
     * @return if < 0, 如果 Q, R, P 小于等于 0
     */
    virtual int utlKalmanFilter(const double& Q, const double& R, const double& x_last, double& x, double& P) = 0;

    /**
     * @brief 卡尔曼滤波器( PVA 模型->状态, 状态的导数及状态的二阶导数组成的线性模型)
     * @param type: 反馈类型,
     *         取值1: 位置反馈, 只需设置 R[0] 和 x 参数
     *         取值2: 位置, 速度反馈, 需要设置 R[0], R[1] 和 x, xd 参数;
     *         取值3: 位置, 速度, 加速度均反馈, 需要设置 R[0], R[1], R[2] 和 x, xd, xdd 参数;
     * @param sample_time: 信号采样周期
     * @param Q: 控制过程协方差
     * @param R: 测量过程协方差, 具体的设定和 type 有关
     * @param x_last: 滤波器在上一时刻的状态输出
     * @param xd_last: 滤波器在上一时刻的状态导数输出
     * @param xdd_last: 滤波器在上一时刻的状态二阶导数输出
     * @param x: 当前需要被滤波的状态及当前输出
     * @param xd: 当前需要被滤波的状态导数及当前输出, 当 type > 1 时, 输入有效
     * @param xdd: 当前需要被滤波的状态导数及当前输出, 当 type > 2 时, 输入有效
     * @param P: 后验估计方差(初值可设为 1)
     * @return if < 0, 如果 sample_time, Q, R, P 的元素小于等于 0 或者 type 取值不为 1,2,3
     */
    virtual int utlKalmanFilterPVA(const int& type, const double& sample_time, const double Q[3], const double R[3], const double& x_last,
    const double& xd_last, const double& xdd_last, double& x, double& xd, double& xdd, double P[9]) = 0;

    /**
     * @brief 巴特沃斯滤波器(无延迟, 适用于离线数据处理)
     * @param order: 滤波器阶数
     * @param fd: 截止频率
     * @param fs: 采样频率
     * @param input: 输入数据(行向量形式)
     * @param output: 输出数据
     * @return if < 0, 则表示计算出错
     */
    virtual int utlButterFilter(const int& order, const double& fd, const double& fs, const DoubleVecVec& input, DoubleVecVec& output) = 0;

    /**
     * @brief utlButterFilter: 巴特沃斯滤波器(无延迟, 适用于离线数据处理)
     * @param order: 滤波器阶数
     * @param cutoff_freq: 截止频率
     * @param sampling_freq: 采样频率
     * @param input: 输入数据(行向量形式)
     * @param filter_axis: 滤波方向，filter_axis = "row", 行滤波；
     * 　　　　　　　　　　　　　　　　filter_axis = "col", 列滤波；
     * 　　　　　　　　　　　　　　　　else 行滤波
     * @param output: 输出数据
     * @return if < 0, 则表示计算出错
     */
    virtual int utlButterFilter(const int order, const double cutoff_freq, const double sampling_freq, const DoubleVecVec& input, const std::string& filter_axis, DoubleVecVec& output) = 0;

    /**
     * @brief 2阶陷波滤波器(无延迟, 仅适用于离线数据处理)
     * @param notch_freq: 陷波中心频率, Hz
     * @param depth: 陷波深度, 体现在bode图的深度值为: 20*log10(depth) dB
     * @param fbb:陷波宽度,Hz
     * @param K: 增益系数, 一般设置为1
     * @param fs: 采样频率, Hz
     * @param input: 输入数据(行向量形式)
     * @param output: 输出数据
     * @return < 0, 初始化2阶陷波滤波器失败
     */
    virtual int utlNotchFilter(const double& notch_freq, const double& depth, const double& fbb, const double& K, const double& fs, const DoubleVecVec& input, DoubleVecVec& output) = 0;

    /**
     * @brief 2阶陷波滤波器(无延迟, 仅适用于离线数据处理)
     * @param notch_freq: 陷波中心频率, Hz
     * @param depth: 陷波深度, 体现在bode图的深度值为: 20*log10(depth) dB
     * @param fbb:陷波宽度,Hz
     * @param K: 增益系数, 一般设置为1
     * @param fs: 采样频率, Hz
     * @param input: 输入数据(行向量形式)
     * @param filter_axis: 滤波方向，filter_axis = "row", 行滤波；
     * 　　　　　　　　　　　　　　　　filter_axis = "col", 列滤波；
     * 　　　　　　　　　　　　　　　　else 行滤波
     * @param output: 输出数据
     * @return < 0, 初始化2阶陷波滤波器失败
     */
    virtual int utlNotchFilter(const double& notch_freq, const double& depth, const double& fbb, const double& K, const double& fs, const DoubleVecVec& input, const std::string& filter_axis, DoubleVecVec& output) = 0;

    /**
     * @brief 2阶陷波滤波器(实时滤波)
     * @param notch_freq: 陷波中心频率, Hz
     * @param depth: 陷波深度, 体现在bode图的深度值为: 20*log10(depth) dB
     * @param fbb:陷波宽度,Hz
     * @param K: 增益系数, 一般设置为1
     * @param fs: 采样频率, Hz
     * @param x_lastlast: 上上个时刻的输入
     * @param x_last: 上个时刻的输入
     * @param x: 当前时刻的输入
     * @param y_lastlast: 上上个时刻的输出
     * @param y_last: 上个时刻的输出
     * @param y: 当前时刻的输出
     * @return < 0, 初始化2阶陷波滤波器失败
     */
    virtual int utlRealTimeNothFilter(const double& notch_freq, const double& depth, const double& fbb, const double& K, const double& fs, const double& x_lastlast, const double& x_last, const double& x, const double& y_lastlast, const double& y_last, double& y) = 0;

    /**
     * @brief 焊接寻位算法，对应需求: http://git.aubo-robotics.cn:8001/aral/aral_export/-/issues/269
     * @param point_in: 输入三个点的位置, point_in[0]为起始点位置, point_in[1]为水平寻位点位置, point_in[2]为垂直寻位点位置
     * @param alpha: 焊点对应两个平面的夹角，属于(0, pi]
     * @param point_out: 输出的寻位点的位置
     * @return if < 0, 则表示计算出错
     */
    virtual int utlSearchPoint(const std::vector<Array3d>& point_in, const double& alpha, Array3d& point_out)const = 0;

    /**
     * @brief 使用N + 1个点定义一个平面: 定义无限平面时 N >= 3，定义有限平面时 N = 3
     * @param points: 三维坐标点构成的矩阵, points = {{x1, y1, z1}, {x2, y2, z2}, ..., {xn, yn, zn}}, points.size()必须大于等于3
     * @param point_direction: 一个三维坐标点，用于规定平面内侧的方向, point_direction = {x0, y0, z0}
     * @param plane: 输出的平面
     * @return ret < 0: 无法定义平面
     */
    virtual int utlDefinePlane(const std::vector<Array3d>& points, const Array3d& point_direction, geometric_shapes::Plane& plane)const = 0;

    /**
     * @brief 计算雅克比矩阵的行列式（用于兼容I系列控制器）
     * @param q_in: 关节角位置
     * @param in_base_or_end: 参考坐标系为基坐标系（或者末端坐标系）
     *        true: 在 基坐标系 下描述
     *        false: 在 末端坐标系 下描述
     * @return 该构型下对应的雅克比矩阵行列式的值
     */
    virtual double utlCalJacobianDeterminant(const RLJntArray& q_in, const bool in_base_or_end) = 0;

    /**
     * @brief 姿态变换函数,　支持　RPY 欧拉角<==>四元数<==>旋转矩阵之间的相互转化, 以及轴角<==>四元数之间的相互转化
     * @param from: 姿态描述类型,　见　OriDescription　结构体
     * @param to: 姿态变换后的类型
     * @param src: 源数据
     * @param dest: 变换后的数据
     */
    virtual int utlTransformOrientation(const OriDescription& from, const OriDescription& to, const double* src, double* dest) const = 0;

    /**
     * @brief 坐标变换(F_c_a = F_c_b * F_b_a)
     * @param F_b_a: a 相对于 b 的位姿
     * @param F_c_b: b 相对于 c 的位姿
     * @return a 相对于 c 的位姿
     */
    virtual RLPose utlChangeReferenceFrame(const RLPose& F_b_a, const RLPose& F_c_b)const = 0;

    /**
     * @brief 改变惯性张量的参考坐标系
     * @param body_a_a: body_a 的转动惯量在坐标系 a　的描述
     * @param F_b_a: a 相对于 b 的位姿
     * @return body_a 的转动惯量在坐标系 b 的描述
     */
    virtual RLInertia utlChangeReferenceFrame(const RLInertia& body_a_a, const RLPose& F_b_a)const = 0;

    /**
     * @brief 计算两个刚体惯性张量的和
     * @param body_a: 刚体 a 的惯性张量在参考坐标系 c 中的描述；
     * @param body_b: 刚体 b 的惯性张量在参考坐标系 c 中的描述；
     * @return  body_a + body_b 的转动惯量在参考坐标系 c 中的描述
     */
    virtual RLInertia utlAddInertia(const RLInertia& body_a, const RLInertia& body_b)const = 0;

    /**
     * @brief 齐次变换矩阵的逆
     * @param F_b_a: a 相对于 b 的位姿
     * @return b 相对于 a 的位姿
     */
    virtual RLPose utlInverseFrame(const RLPose& F_b_a)const = 0;

    /**
     * @brief 计算两个 pose 间的偏差
     * @param F_a_b1: b1 相对于 a 的位姿
     * @param F_a_b2: b2 相对于 a 的位姿
     * @return v_a = F_a_b2 - F_a_b1; 速度在 a 坐标系的描述
     */
    virtual RLTwist utlCalDifferPose(const RLPose& F_a_b1, const RLPose& F_a_b2)const = 0;

    /**
     * @brief 改变 RLWrench 或　RLTwist　的参考坐标系(RLWrench 和　RLTwist变换方式一致)
     * @param vector_in_a: 输入作用在 a 点的 vector 在 a 坐标系描述
     * @param F_b_a: a 相对于 b 的位姿
     * @return: 输出作用在 a 点的 vector 在 b 坐标系描述
     */
    virtual Array6d utlChangeSpatialVectorBase(const Array6d& vector_in_a, const RLPose& F_b_a)const = 0;

    /**
     * @brief 改变　RLWrench　或　RLTwist　的作用点和参考坐标系(RLWrench 和　RLTwist变换方式一致)
     * @param vector_in_a: 输入作用在 a 点的 vector 在 a 坐标系描述
     * @param F_b_a: a 相对于 b 的位姿
     * @return: 输出作用在 b 点的 vector 在 b 坐标系描述
     */
    virtual Array6d utlChangeSpatialVectorFrame(const RLWrench& vector_in_a, const RLPose& F_b_a)const = 0;

    /**
     * @brief 改变向量的参考坐标系
     * @param V_in_a: 输入向量 V　在 a 坐标系描述
     * @param F_b_a: a 相对于 b 的位姿
     * @return: 输出向量　V　在 b 坐标系描述
     */
    virtual Array3d utlChangeVectorBase(const Array3d& V_in_a, const RLPose& F_b_a)const = 0;

    /**
     * @brief 参考坐标系的变换, 此接口为应用实现.
     *        参考:　http://git.aubo-robotics.cn:8001/aral_application/aral_export/-/issues/2
     * @param F_b_a_old: old 参考坐标系在　基坐标系 的描述
     * @param V_in_a: 新的坐标系的方向向量(不需要单位化)
     * 　　　　if type = 1; 为新的 ｙ 轴的方向向量在　old 参考坐标系的描述
     * 　　　　if type = 2; 为速度方向向量在　old 参考坐标系的描述
     * @param type: 参考上面定义
     * @param F_b_a_new: 变换之后的 new 参考坐标系在　基坐标系 的描述
     * @return if < 0; something goes wrong
     */
    virtual int utlTransferReferenceFrame(const RLPose& F_b_a_old, const Array3d& V_in_a, const int& type, RLPose& F_b_a_new)const = 0;

    /**
     * @brief 计算以给定速度变换单位时间后的位姿
     * @param F_w_a: 当前时刻　a 相对于 w 的位姿
     * @param da_w: 当前时刻　a 坐标系的速度在　w 的描述
     * @return 单位时间后的位姿在 w 的描述
     */
    virtual RLPose utlAddDeltaPose(const RLPose& F_w_a, const RLTwist& da_w)const = 0;

    /**
     * @brief 计算以给定速度变换 t 时刻后的姿态
     * @param R_w_a: 当前时刻　a 相对于 w 的姿态
     * @param ra_w: 当前时刻　a 的角速度在　w 的描述
     * @param dt: 速度持续的时间
     * @return 变化后的姿态在 w 的描述
     */
    virtual Array3d utlAddDeltaOri(const Array3d& R_w_a, const Array3d& ra_w, const double& dt)const = 0;

    /**
     * @brief 添加物体(Object)到世界环境(World)
     * @param object_id: 物体名称
     * @param pose: 描述物体坐标系相对于世界坐标系位姿
     * @param shapes: 物体的形状集合
     * @param origins: 描述物体的各形状相对于物体坐标系位姿
     * @return if < 0, 则表示设置出错
     */
    virtual int utlSetWorldObject(const std::string& object_id, const RLPose& pose, const std::vector<geometric_shapes::ShapeConstPtr>& shapes, const std::vector<RLPose>& origins) = 0;

    /**
     * @brief 获取世界环境(World)中的物体
     * @param object_id: 物体名称
     * @param pose: 描述物体坐标系相对于世界坐标系位姿
     * @param shapes: 物体的形状集合
     * @param origins: 描述物体的各形状相对于物体坐标系位姿
     * @return if < 0, 则表示获取出错
     */
    virtual int utlGetWorldObject(const std::string& object_id, RLPose& pose, std::vector<geometric_shapes::ShapeConstPtr>& shapes, std::vector<RLPose>& origins) = 0;

    /**
     * @brief 移动世界环境(World)中的物体
     * @param object_id: 物体名称
     * @param pose: 描述物体坐标系相对于世界坐标系位姿
     * @return if < 0, 则表示计算出错
     */
    virtual int utlMoveWorldObject(const std::string& object_id, const RLPose& pose) = 0;

    /**
     * @brief 删除世界环境(World)中的物体(Object)
     * @param object_id: 物体名称
     * @return if < 0, 则表示设置出错
     */
    virtual int utlUnsetWorldObject(const std::string& object_id) = 0;
     
};
typedef std::shared_ptr<Utility> UtilityPtr;

} // namespace ARAL::interface

#endif // ARAL_INTERFACE_UTILITY_HPP